#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口�?
#include <std_msgs/String.h>
#include <std_msgs/Empty.h> 
#include <string>
#include <vector>
#include <sstream>
#include <cmath>
#include <cstdlib>  //string转化为double
#include <iomanip>  //保留有效小数
#include<iostream>  
#include <custom_msg/CMD.h>
#include "commons/rabbit_consts.h"
#include <stdint.h>

using namespace std;

static unsigned char rec_buf[SERAIL_READ_MAX_LEN];
serial::Serial ser; //声明串口对象
custom_msg::CMD CMD_data;  //全局变量，解析后数据

//new device benewake 0--350m 
int main(int argc, char** argv) {
  int len,focus=0,hw_rgb=0,val_type=101,minLen = 0;
  char val_char[50]={0x00};
  ros::init(argc, argv, "serial_node_tc");
  ros::NodeHandle nh;
  ros::Publisher CMD_pub = nh.advertise<custom_msg::CMD>(TOPIC_CMD, 1000);
  try
  {
    ser.setPort(SERIAL_TC_DEV);
    ser.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();
  }
  catch (serial::IOException& e)
  {
    ROS_ERROR_STREAM("Unable to open Serial Port !");
    return -1;
  }
  if (ser.isOpen())
  {
    ROS_INFO_STREAM("Serial Port initialized");
  }
  else
  {
    return -1;
  }
  ros::Rate loop_rate(50);
  while (ros::ok()) {
    len = ser.available();
    minLen=min(len, SERAIL_READ_MAX_LEN);
    if (minLen >= 30) {
       try
       {
         memset(rec_buf, 0x00, SERAIL_READ_MAX_LEN);
         len = ser.read(rec_buf , minLen); 
       }
       catch (serial::IOException& e)
       {
         ROS_ERROR_STREAM("read Serial Port fail!");
         continue;
       }
      if(*(rec_buf)==0x0d){
        focus = *(uint16_t*)(rec_buf+11);
        hw_rgb = *(rec_buf+17);
        //cout<<"minLen ::"<<minLen<<  " focus ::"<<focus<< " hw_rgb ::"<<hw_rgb<<  "\r\n"; 
        CMD_data.cmd_type = 1;
        CMD_data.header.stamp = ros::Time::now();
        if(hw_rgb==1){ //hw
           CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_HW;
           CMD_pub.publish(CMD_data);     
        }else{
          CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_RGB;
          CMD_pub.publish(CMD_data);  
       }
        if(*(rec_buf+29)==0x0a){
          if(focus<=10000){
            if(hw_rgb==1){ //hw
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_HW_FOCUS1;
            }else{
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS1;
            }
          }else if(focus>10000 && focus<=20000){
            if(hw_rgb==1){ //hw
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_HW_FOCUS2;
            }else{
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS2;
            }
          }else if(focus>20000 && focus<=30000){
            if(hw_rgb==1){ //hw
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_HW_FOCUS3;
            }else{
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS3;
            }
          }else if(focus>40000 && focus<=40000){
            if(hw_rgb==1){ //hw
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_HW_FOCUS4;
            }else{
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS4;
            }
          }else if(focus>40000 && focus<=50000){
            if(hw_rgb==1){ //hw
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_HW_FOCUS5;
            }else{
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS5;
            }
          }else{
            if(hw_rgb==1){ //hw
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_HW_FOCUS6;
            }else{
              CMD_data.val_type = MSG_CMD_TYPE_SUMBOY_RGB_FOCUS6;
            }
          }
          CMD_pub.publish(CMD_data);
        }
        continue;
      }
  }
    ros::spinOnce();
    loop_rate.sleep();
  }
}

